#pragma once
//lane test
#include "lane_manager.h"
#include "lane_detection.h"
#include "GtrackViewer.h"
#include "FileHandler.h"
#include <chrono>
#include <iostream>
#include "mgl2/mgl.h"
static Point2D bezier_calculate(const BezierCurve* curve, float t)
{
    float u = 1.0f - t;
    float x_ = u * u * curve->p0.x + 2 * u * t * curve->p1.x + t * t * curve->p2.x;
    float y_ = u * u * curve->p0.y + 2 * u * t * curve->p1.y + t * t * curve->p2.y;
    Point2D Position = { x_,y_ };
    return Position;
}
#include <stdlib.h>
#include <stdio.h>
#include <time.h>

Point2D getRandonm(int randRangeX, float BASE_X, int randRangeY, float BASE_Y)
{
    float X = rand() % randRangeX + BASE_X;
    float Y = rand() % randRangeY + BASE_Y;
    Point2D A = { X,Y };
    return A;
}


int test()
{
    int srand((unsigned)time(NULL));  // 仅需在程序启动时调用一次[3,4](@ref)
    //预设一个很大范围的画图
    mglGraph* gr = new mglGraph(0, 2000, 2000);
    gr->SetRanges(-50, 40, 0, 300);	    // 设置坐标轴范围
    gr->Axis();							// 显示坐标轴
    gr->Label('x', "x", 0);
    gr->Label('y', "y", 0);
    std::string pen_str = "r. ";

    //mglData cx(1000), cy(1000); //预设俩大的
    //mglData bx(1000), by(1000);


    ////测试
    RoadTopology road;
    uint8_t roadNumbers = 6; //车道线数量6
    float width = 3.f; //车道宽度设置
    BezierCurve a[3]; //曲线段 这里2个
    a[0] = { {0,0}, {5,84} ,{10,100} };
    a[1] = { {10,100},{0,120}, {13,220} };
    a[2] = { {13,220},{0,250}, {13,320} };

    //生成车道线/ 可向左可向右
     
    //road_add_lane(&road, roadNumbers, a, 3, width, INSERT_LEFT); 
    //road_add_lane(&road, roadNumbers, a, 2, width, INSERT_RIGHT);
    //保存图片的地址
    std::string fileName = "F:\\gtracker_ws\\gtrack\\eventDetection\\曲率符合国标\\车道线查询.png";
    //保存文件的地址
    std::string dataName = "F:\\gtracker_ws\\gtrack\\eventDetection\\曲率符合国标\\车道识别.csv";
#pragma region 测试修改某条车道线
    //road_adjust_width(&road, 0, 5.0f); // 修改特定车道宽度 0等于第一个车道
    //road_adjust_width(&road, 0, 1.0f); 

        //for (int i = 0; i < road.count; i++)
        //{
        //    Point2D pts;
        //    int m = 0;
        //  /*  if (i > 0)
        //        pen_str = "b. ";*/
        //    for (int line = 0; line < 3; line++) //2段曲线
        //    {
        //        for (float t = 0.0; t < 1.0; t += 0.01)
        //        {
        //            pts = bezier_calculate(&road.lanes[i].curve[line], t);
        //            //float qulv = bezier_curvature(road.lanes[i].curve[0].p0, road.lanes[i].curve[0].p1, road.lanes[i].curve[0].p2,t);
        //            cx.a[m] = pts.x;
        //            cy.a[m] = pts.y;
        //            m++;
        //        }
        //       
        //        for (int t = 2; t < 300; t++) //查表范围
        //        {
        //            bx.a[t] = road.lanes[i].y2x[t];
        //            by.a[t] = t;
        //        }
        //        
        //    }
        //    pen_str = "r. ";
        //    gr->Plot(cx, cy, pen_str.c_str());
        //    pen_str = "b* ";
        //    gr->Plot(bx, by, pen_str.c_str());
        //    
        //}


        //gr->WritePNG(fileName.c_str());
#pragma endregion


#pragma region 存储过程数据
     //FILE* fp = fopen(dataName.c_str(), "w");
     //float x, y;
     //for (int i = 0; i < road.count; i++)
     //{

     //        for (int t = 10; t < 90; t++)
     //        {
     //            x = road.lanes[i].y2x[t];
     //            y = (float)t;
     //            fprintf(fp, "%d,%f,%f\n", i, x, y);
     //        }
     //  /*      for (float t = 0.0; t < 1.0; t += 0.01)
     //        {
     //            Point2D pts = bezier_calculate(&road.lanes[i].curve[0], t);
     //            float qulv = bezier_curvature(road.lanes[i].curve[0].p0, road.lanes[i].curve[0].p1, road.lanes[i].curve[0].p2, t); 算曲率的
     //            x = pts.x;
     //            y = pts.y;
     //            fprintf(fp, "%d,%f,%f,%f \n", i, x, y,qulv);
     //        }*/
     //}

     //

     //fclose(fp);
#pragma endregion

#pragma region 测试在哪个车道

#include <iostream>
#include <string>



for (int RANDNUM = 0; RANDNUM < 10; RANDNUM++)
{

    Point2D p[7];
     p[0] = getRandonm(5,0,1,0);
     p[1] = getRandonm(10,0, 20,30);
     p[2] = getRandonm(10, 0, 10, 100);
     p[3] = getRandonm(10, 0, 10, 150);
     p[4] = getRandonm(10, 0, 10, 200);
     p[5] = getRandonm(10, 0, 10, 250);
     p[6] = getRandonm(10, 0, 10, 300);
    
    BezierCurve a[3]; //曲线段 这里2个
    a[0] = { p[0], p[1] ,p[2] };
    a[1] = { p[2],p[3] , p[4] };
    a[2] = { p[4], p[5],  p[6] };

    //生成车道线/ 可向左可向右

    road_add_lane(&road, roadNumbers, a, 3, width, INSERT_RIGHT);
    road_adjust_width(&road, 2, 5.0f); 
    mglData cx(1000), cy(1000); //预设俩大的
    mglData bx(1000), by(1000);
    for (int i = 0; i < road.count+1; i++)
    {
        //Point2D pts;
        //int m = 0;
        /*  if (i > 0)
            pen_str = "b. ";*/
        //for (int line = 0; line < 3; line++) //2段曲线
        //{
        //    for (float t = 0.0; t < 1.0; t += 0.01)
        //    {
        //        pts = bezier_calculate(&road.lanes[i].curve[line], t);
        //        //float qulv = bezier_curvature(road.lanes[i].curve[0].p0, road.lanes[i].curve[0].p1, road.lanes[i].curve[0].p2,t);
        //        cx.a[m] = pts.x;
        //        cy.a[m] = pts.y;
        //        m++;
        //    }
        //       
       
        //}
        for (int t = 2; t < 300; t++) //查表范围
        {
            bx.a[t] = road.lanes[i].y2x[t];
            by.a[t] = t;
        }
      /*  pen_str = "r. ";
        gr->Plot(cx, cy, pen_str.c_str());*/
        pen_str = "b. ";
        gr->Plot(bx, by, pen_str.c_str());
            
    }
   

     //车道线测试
    Point2D carPose = getRandonm(25, 0, 300, 0);

    pen_str = "r* ";
    gr->Mark(mglPoint(carPose.x, carPose.y), pen_str.c_str());
    int roadline = detect_current_lane(&road, &carPose);
   
    fileName = "F:\\gtracker_ws\\gtrack\\eventDetection\\曲率符合国标\\" +std::to_string(RANDNUM) + "车道线查询.png";
    char c_clusterID[256];
    snprintf(c_clusterID, 256, "%d", roadline);
    gr->Puts(mglPoint(carPose.x + 2, carPose.y + 2), c_clusterID);
    
 
    printf("处于第%d个车道\n", roadline);
    printf("存储图片中,别急\n");
    gr->Axis();							// 显示坐标轴
    gr->Box();							// 显示图片框
    gr->Grid("xy", "k");
    gr->Label('x', "x", 0);
    gr->Label('y', "y", 0);

    gr->WritePNG(fileName.c_str());
    printf("存储完成\n");
    gr->Clf();
#pragma endregion
}
    delete gr;

    return 0;
}